/*
 * host.c
 *
 *  Created on: Oct 16, 2024
 *      Author: Jinming
 */
//#include <stddef.h>
#include "sensor.h"
#include <string.h>
#include "host.h"
#include "cmsis_os.h"
#include "task_info.h"
#include "motor_task.h"
#include "spi.h"
#include "tim.h"
#include "led.h"

#define TAG		"Host"
#define SPI_HANDLE	hspi1

#define SPI_DMA_BUFFER_LEN		300

static uint8_t gs_RxBuffer[SPI_DMA_BUFFER_LEN];
static uint16_t gs_hRxLen = 0;

osThreadId_t g_tHostTask = NULL;
const osThreadAttr_t g_tHostTaskAttr = {
  .name = HOST_TASK_NAME,
  .stack_size = 128 * 8,
  .priority = (osPriority_t) HOST_TASK_PRIO,
};

osSemaphoreId_t g_tHostSem = NULL;
const osSemaphoreAttr_t g_tHostSemAttr = {
  .name = "host"
};

typedef union
{
	spi_command_t tCmd;
	uint8_t buffer[SPI_DMA_BUFFER_LEN];
}SpiDataCommand_t;

static uint8_t gs_tTxBuffer[SPI_DMA_BUFFER_LEN];
static SpiDataCommand_t gs_tRxBuffer;
uint8_t cRxtmp;

static SUB_DEV_e gs_eSubDev = SUB_DEV_NONE;
static spine_data_t gs_tSpiData;       // data from spine to up
static JointCtrl_t gs_tJointCtrl[JOINT_NUM];

/* 数据校验 */
static uint8_t checksum(uint8_t *data, uint16_t len)
{
    uint8_t res = 0;

    for (int i = 0; i < len; i++)
    {
        res = res ^ data[i];
    }

    return res;
}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
	if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_4) == 0)
	{
		cRxtmp = SPI_HANDLE.Instance->DR;

		  if ((SPI_HANDLE.Instance->CR1 & SPI_CR1_SPE) != SPI_CR1_SPE)
		  {
		    /* Enable SPI peripheral */
		    __HAL_SPI_ENABLE(&SPI_HANDLE);
		  }
		  /* Enable the SPI Error Interrupt Bit */
		  __HAL_SPI_ENABLE_IT(&SPI_HANDLE, (SPI_IT_ERR));
		SET_BIT(SPI_HANDLE.Instance->CR2, SPI_CR2_RXDMAEN);
		SET_BIT(SPI_HANDLE.Instance->CR2, SPI_CR2_TXDMAEN);
	}
	else if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_4) == 1)
	{
		gs_hRxLen = SPI_DMA_BUFFER_LEN - SPI_HANDLE.hdmarx->Instance->NDTR;
		HAL_SPI_DMAStop(&SPI_HANDLE);
		memcpy(gs_tRxBuffer.buffer, gs_RxBuffer, gs_hRxLen);
//		log_buf(gs_tRxBuffer.buffer, gs_hRxLen);
		osSemaphoreRelease(g_tHostSem);
	}
}

int motor_spi_query_read(spi_command_t *cmd)
{
    if (cmd->cmdType == SUB_DEV_QUERY_JOINT)
    {
        if (gs_eSubDev != SUB_DEV_JOINT) 	// 第一次
        {
        	gs_eSubDev = SUB_DEV_JOINT;
        	led_set(0, 25, 50);
        }
        motor_dev_update();
        return 0;
    }
    if (cmd->cmdType == SUB_DEV_QUERY_SENSOR)
    {
        if (gs_eSubDev != SUB_DEV_SENSOR) // 第一次
        {
        	sensor_config();
        	led_set(0, 20, 100);
            gs_eSubDev = SUB_DEV_SENSOR;
        }
        return 0;
    }

    return -1;
}

JointCtrl_t *joint_command_get(void)
{
	return gs_tJointCtrl;
}

void joint_command_clear(void)
{
	memset(gs_tJointCtrl, 0, sizeof(JointCtrl_t)*JOINT_NUM);
	for(uint8_t i = 0; i < JOINT_NUM; i++)
	{
		gs_tJointCtrl[i].type = CAN_PACKET_SET_IDLE;
	}
}

void update_leg_command_to_control()
{
    memset(gs_tJointCtrl, 0, sizeof(gs_tJointCtrl));

    if(gs_eSubDev == SUB_DEV_JOINT)
    {
    	for(int i = 0; i < JOINT_NUM; i++)
    	{
    		gs_tJointCtrl[i].p_des = gs_tRxBuffer.tCmd.motor[i].pos;
    		gs_tJointCtrl[i].v_des = gs_tRxBuffer.tCmd.motor[i].speed;
    		gs_tJointCtrl[i].a_des = gs_tRxBuffer.tCmd.motor[i].acc;
    		gs_tJointCtrl[i].kp_des = gs_tRxBuffer.tCmd.motor[i].kp;
    		gs_tJointCtrl[i].ki_des = gs_tRxBuffer.tCmd.motor[i].ki;
    		gs_tJointCtrl[i].kd_des = gs_tRxBuffer.tCmd.motor[i].kd;
    		gs_tJointCtrl[i].tff_des = gs_tRxBuffer.tCmd.motor[i].torque;
    		gs_tJointCtrl[i].type = gs_tRxBuffer.tCmd.motor[i].type;
//    		LOGD(TAG, "joint%d: %ld\t%ld\t%ld", i, gs_tJointCtrl[i].p_des,
//    				gs_tJointCtrl[i].v_des, gs_tJointCtrl[i].type);
    	}
    }
}

void spi_tx_buffer_sync(void)
{
	uint16_t hPos = 0;

	if(gs_eSubDev == SUB_DEV_JOINT)
	{
		gs_tTxBuffer[hPos++] = 0xA5;
		gs_tTxBuffer[hPos++] = sizeof(gs_tSpiData.motor) + 2;
		gs_tTxBuffer[hPos++] = SUB_DEV_JOINT;
		memcpy(&gs_tTxBuffer[hPos], gs_tSpiData.motor, sizeof(gs_tSpiData.motor));
		hPos += sizeof(gs_tSpiData.motor);
		gs_tTxBuffer[hPos] = checksum(gs_tTxBuffer, hPos);
	}
	else if(gs_eSubDev == SUB_DEV_SENSOR)
	{
		gs_tTxBuffer[hPos++] = 0xA5;
		gs_tTxBuffer[hPos++] = sizeof(gs_tSpiData.sensor) + 2;
		gs_tTxBuffer[hPos++] = SUB_DEV_SENSOR;
		memcpy(&gs_tTxBuffer[hPos], &gs_tSpiData.sensor, sizeof(gs_tSpiData.sensor));
		hPos += sizeof(gs_tSpiData.sensor);
		gs_tTxBuffer[hPos] = checksum(gs_tTxBuffer, hPos);
	}
}

void host_to_spi_sync(MotorInfo_t *motorInfo, uint8_t index)
{
	if(gs_eSubDev == SUB_DEV_JOINT)
	{
		gs_tSpiData.motor[index].pos = motorInfo->p;
		gs_tSpiData.motor[index].speed = motorInfo->v;
		gs_tSpiData.motor[index].current = motorInfo->t;
		gs_tSpiData.motor[index].temp = motorInfo->temperature;
		gs_tSpiData.motor[index].errCode = motorInfo->errorcode;
//		LOGD(TAG, "%ld\t%ld\t%ld", motor[0].info.p, motor[0].info.v,
//				motor[0].info.t);
	}
	else if(gs_eSubDev == SUB_DEV_SENSOR)
	{
		sensor_get(&gs_tSpiData.sensor);
	}
}

/* SPI线程处理接收指令任务 */
void host_task(void *param)
{
	osStatus_t status;
	uint16_t hCheckLen;
	uint32_t lCheckCode;

	SPI_TransmitReceive_DMA(&SPI_HANDLE, gs_tTxBuffer, \
			gs_RxBuffer, SPI_DMA_BUFFER_LEN);

	while(1)
	{
		status = osSemaphoreAcquire(g_tHostSem, portMAX_DELAY);
		if(status)
		{
			LOGD(TAG, "host sem acquire fail");
			continue;
		}

//		log_buf(gs_tRxBuffer.buffer, gs_hRxLen);
//		LOGD(TAG, "gs_hRxLen=%d", gs_hRxLen);
		if(gs_tRxBuffer.buffer[0] == 0xA5)		//检测到头码
		{
			hCheckLen = gs_tRxBuffer.buffer[1];
			lCheckCode = checksum(gs_tRxBuffer.buffer, hCheckLen + 1);
			if(lCheckCode == gs_tRxBuffer.buffer[hCheckLen + 1])		//校验通过
			{
				host_lost_timer_update(1500);
				if(motor_spi_query_read(&gs_tRxBuffer.tCmd) == 0)		//设置端口
				{
//					LOGD(TAG, "set port");
					start_time3(1200);			//获取电机状态信息倒计时;
				}
				else if(gs_tRxBuffer.buffer[2] == SUB_DEV_JOINT)
				{
//					LOGE(TAG, "motor");
					start_time3(1200);			//获取电机状态信息倒计时
					update_leg_command_to_control();
					host_command_to_motor();
				}
				else if(gs_tRxBuffer.buffer[2] == SUB_DEV_SENSOR)
				{
//					LOGE(TAG, "sensor");
					start_time3(1200);;
				}
				else
				{
					LOGE(TAG, "unknow receive type %d", gs_tRxBuffer.buffer[2]);
				}
			}
			else
			{
//				LOGD(TAG, "gs_hRxLen=%d", gs_hRxLen);
				LOGE(TAG, "check error");
			}
		}
		else
		{
			LOGE(TAG, "can not find head type");
		}

		memset(gs_tRxBuffer.buffer, 0, gs_hRxLen);
		gs_hRxLen = 0;

		spi_tx_buffer_sync();

	    SPI_TransmitReceive_DMA(&SPI_HANDLE, gs_tTxBuffer, \
	    						gs_RxBuffer, SPI_DMA_BUFFER_LEN);
	}
}

void host_lost_timerout(void)
{
	host_lost_to_motor();
}

/* 主机通信初始化 */
int host_task_init(void)
{
	g_tHostSem = osSemaphoreNew(1, 0, &g_tHostSemAttr);
	if(g_tHostSem == NULL)
	{
		LOGE(TAG, "Host sem create error!!!");
		return -1;
	}

	g_tHostTask = osThreadNew(host_task, NULL, &g_tHostTaskAttr);
	if(g_tHostTask == NULL)
	{
		LOGE(TAG, "Host task create error!!!");
		return -1;
	}

	return 0;
}
